
import matplotlib.pyplot as plt
import numpy as np
import math

T = 1
K = [1,2,3,4]
Pk = np.array([[99999999,0,0],[0,99999999,0],[0,0,99999999]])
φk = np.array([[1,T,0.5 * T**2],[0,1,T], [0,0,1]])
H = np.array([[1,0,0]])
Rk = 4.0 #都为标量
xhat_prev = np.array([[0],[0],[0]])
xhat = np.array([[0],[0],[0]])

x_measure = [0] * 4
t = [0] * 4
print('请输入：')
for i in range(0,4):
    a = input()
    a = float(a)
    x_measure[i] = a
    t[i] = (K[i] - 1) * T
print('输入的是：', x_measure)

for i in range(0,4):
    φk_T = np.transpose(φk)
    H_T = np.transpose(H)
    Mk = φk.dot(Pk).dot(φk_T)                    #2*2
    CC = np.linalg.inv(H.dot(Mk).dot(H_T)+Rk)
    Kk = Mk.dot(H_T).dot(CC)                     #2*1
    Pk = (np.eye(3) - Kk.dot(H)).dot(Mk)         #2*2
    x = φk.dot(xhat_prev) + Kk.dot(x_measure[i]-H.dot(φk).dot(xhat_prev))
    xhat_prev = x
    xhat = np.c_[xhat, x]  # np.c_是按行连接两个矩阵，就是把两矩阵左右相加，要求行数相等。
xhat = np.delete(xhat, 0, axis=1) #删除第0列

plt.xlabel("Time(Sec)")
plt.ylabel("xhat")
plt.plot(t,x_measure,'+')
plt.plot(t,xhat[0], c="k",label = 'estimates')
plt.legend()
plt.show()

